#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <helper/helper_utils.h>

using namespace std;

// 运行示例： rosrun helper helper_imu_info /imu

/**
 * @description: 
 * @return {*}
 */
class ImuInfo{
    public:
        ImuInfo();
        ImuInfo(string imu_topic);
        ~ImuInfo();

    private:
        ros::NodeHandle __nh;
        ros::Subscriber __imuSub;

        // imu回调函数
        void ImuCallback(const sensor_msgs::ImuConstPtr& msg);

};


/**
 * @description: 
 * @return {*}
 */
ImuInfo::ImuInfo(){
    __imuSub = __nh.subscribe("/imu", 1, &ImuInfo::ImuCallback, this);
}

ImuInfo::ImuInfo(string imu_topic){
    __imuSub = __nh.subscribe(imu_topic, 1, &ImuInfo::ImuCallback, this);
}

/**
 * @description: 
 * @return {*}
 */
ImuInfo::~ImuInfo(){
    
}


/**
 * @description: 
 * @param {ImuConstPtr&} msg
 * @return {*}
 */
void ImuInfo::ImuCallback(const sensor_msgs::ImuConstPtr& msg){
    // 打印imu数据 打印四元数转欧拉角，线加速度和角速度
    ROS_INFO("----------------------[ImuCallback]----------------------");
    // 将四元数转为欧拉角
    Eigen::Vector3d euler = helper::quaternionToEuler(msg->orientation);
    ROS_INFO("[%s] euler: (%.3f, %.3f, %.3f), deg: (%.3f, %.3f, %.3f), linear_acceleration: (%.3f, %.3f, %.3f), angular_velocity: (%.3f, %.3f, %.3f)",
        helper::stamp_to_standard(msg->header.stamp.sec).c_str(),
        euler(0), euler(1), euler(2),
        helper::Rad2Deg(euler(0)), helper::Rad2Deg(euler(1)), helper::Rad2Deg(euler(2)),
        msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,
        msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z
    );
}


/**
 * @description: 
 * @param {int} argc
 * @param {char} *
 * @return {*}
 */
int main(int argc, char **argv ){

    // 打印参数
    for(int i=0; i<argc; i++){
        std::cout << "i: " << i << ", data: " << argv[i] << std::endl;
    }
    if(argc < 2){
        std::cout << "please set imu topic."  << std::endl;
        return 1;
    }
    ros::init(argc, argv, "helper_imu_info");
    ROS_INFO("helper_imu_info initialization ......");
    ros::NodeHandle nh;
    ImuInfo imu_info(argv[1]);
    ros::spin();
    return 0;
}